#include "sci_lio/faster_voxel_grid.h"

std::mutex mtx; // 创建互斥锁
//! 创新点：修复了一个重大的Bug
size_t FasterVoxelGrid::ComputeHashIndex(const Eigen::Vector3d& point, Eigen::Matrix3d R_w_be, Eigen::Vector3d t_w_be) 
{
  const Eigen::Vector3d point_W = R_w_be * point + t_w_be;

  double loc_xyz[3];
  for (size_t i = 0; i < 3; ++i) 
  {
    loc_xyz[i] = point_W[i] * inv_resolution_;
    if (loc_xyz[i] < 0) {
      loc_xyz[i] -= 1.0;
    }
  }

  size_t x = static_cast<size_t>(loc_xyz[0]);
  size_t y = static_cast<size_t>(loc_xyz[1]);
  size_t z = static_cast<size_t>(loc_xyz[2]);

  return ((((z)*HASH_P_) % MAX_N_ + (y)) * HASH_P_) % MAX_N_ + (x);
}
void FasterVoxelGrid::GetGridXY(const Eigen::Vector3d& point, Eigen::Matrix3d R_w_be, Eigen::Vector3d t_w_be, size_t& x, size_t& y)
{
  const Eigen::Vector3d point_W = R_w_be * point + t_w_be;
  double loc_xyz[3];
  for (size_t i = 0; i < 3; ++i) 
  {
    loc_xyz[i] = point_W[i] * inv_resolution_;
    if (loc_xyz[i] < 0) {
      loc_xyz[i] -= 1.0;
    }
  }

  x = static_cast<size_t>(loc_xyz[0]);
  y = static_cast<size_t>(loc_xyz[1]);
}

size_t FasterVoxelGrid::ComputeMaxZHashIndex(const Eigen::Vector3d& point, Eigen::Matrix3d R_w_be, Eigen::Vector3d t_w_be,double& cur_z) 
{
  const Eigen::Vector3d point_W = R_w_be * point + t_w_be;

  double loc_xyz[3];
  for (size_t i = 0; i < 3; ++i) 
  {
    loc_xyz[i] = point_W[i] * inv_resolution_;
    if (loc_xyz[i] < 0) {
      loc_xyz[i] -= 1.0;
    }
  }

  size_t x = static_cast<size_t>(loc_xyz[0]);
  size_t y = static_cast<size_t>(loc_xyz[1]);
  size_t z = 0;

  cur_z = point_W[2];

  return ((((z)*HASH_P_) % MAX_N_ + (y)) * HASH_P_) % MAX_N_ + (x);
}

size_t FasterVoxelGrid::ComputeHashIndex(const Eigen::Vector3d& point) 
{
  double loc_xyz[3];
  for (size_t i = 0; i < 3; ++i) 
  {
    loc_xyz[i] = point[i] * inv_resolution_;
    if (loc_xyz[i] < 0) {
      loc_xyz[i] -= 1.0;
    }
  }

  size_t x = static_cast<size_t>(loc_xyz[0]);
  size_t y = static_cast<size_t>(loc_xyz[1]);
  size_t z = static_cast<size_t>(loc_xyz[2]);

  return ((((z)*HASH_P_) % MAX_N_ + (y)) * HASH_P_) % MAX_N_ + (x);
}

void FasterVoxelGrid::Filter(const CloudPtr& input_cloud_ptr,
                             CloudPtr& cloud_DS_ptr,
                             CloudCovPtr& cloud_cov_ptr, Eigen::Matrix3d R_w_be, Eigen::Vector3d t_w_be) 
{
  voxel_array_ptr_->clear();
  voxel_map_MaxZ_ptr_->clear();
  voxel_map_ptr_->clear();
  max_z_map_.clear();
  min_z_map_.clear();

  voxel_array_ptr_->reserve(input_cloud_ptr->size());
  
  //把每个点插入到体素里面，计算每个体素的质心centorid_和体素内点的数量N_
  tbb::parallel_for(tbb::blocked_range<size_t>(0, input_cloud_ptr->size()),[&, this](tbb::blocked_range<size_t> r) 
  {
    for (size_t i = r.begin(); i < r.end(); ++i) 
    {
      //获取每个点的坐标
      const Eigen::Vector3d point =input_cloud_ptr->points[i].getVector3fMap().template cast<double>();
      //计算该点对应的hash索引
      size_t hash_idx = ComputeHashIndex(point,R_w_be,t_w_be);

      double cur_z{0};
      size_t hash_idx_z = ComputeMaxZHashIndex(point,R_w_be,t_w_be,cur_z);//令z=0,计算hash_z索引
      MyGrid MaxZ_MyGrid;
      size_t x;
      size_t y;
      GetGridXY(point,R_w_be,t_w_be,x,y);
      MaxZ_MyGrid.x = x;
      MaxZ_MyGrid.y = y;

      MyAccessor accessor;
      voxel_map_ptr_->insert(accessor, hash_idx);
      //如果grid不存在，则创建
      if (accessor->second == nullptr) 
      {
        accessor->second = std::make_shared<Voxel>();

        accessor->second->centorid_ = point;
        accessor->second->N_++;

        voxel_array_ptr_->emplace_back(accessor->second);
      }
      //如果grid存在，则更新
      else 
      {
        accessor->second->N_++;
        accessor->second->centorid_ += (point - accessor->second->centorid_)/static_cast<double>(accessor->second->N_);
      }

      //! 创新点：最大高度的hash地图
      MyAccessor MaxZ_accessor;
      voxel_map_MaxZ_ptr_->insert(MaxZ_accessor, hash_idx_z);
      if (MaxZ_accessor->second == nullptr) 
      {
        MaxZ_accessor->second = std::make_shared<Voxel>();
        MaxZ_accessor->second->max_z = cur_z;
        MaxZ_accessor->second->min_z = cur_z;
        MaxZ_accessor->second->maxZ_hash_index_ = hash_idx;
        MaxZ_accessor->second->minZ_hash_index_ = hash_idx;
        std::lock_guard<std::mutex> lock(mtx);
        max_z_map_.insert(std::make_pair(MaxZ_MyGrid, cur_z));
        min_z_map_.insert(std::make_pair(MaxZ_MyGrid, cur_z));
      }
      //如果grid存在，则更新
      else 
      {
        //更新最高点索引
        if(cur_z > MaxZ_accessor->second->max_z)
        {
          MaxZ_accessor->second->max_z = cur_z;
          MaxZ_accessor->second->maxZ_hash_index_ = hash_idx;
          std::lock_guard<std::mutex> lock(mtx);
          max_z_map_ [MaxZ_MyGrid] = cur_z;
        }
        //更新最低点索引
        if(cur_z < MaxZ_accessor->second->min_z)
        {
          MaxZ_accessor->second->min_z = cur_z;
          MaxZ_accessor->second->minZ_hash_index_ = hash_idx;
          std::lock_guard<std::mutex> lock(mtx);
          min_z_map_ [MaxZ_MyGrid] = cur_z;
        }
      }
      //LOG(INFO) << "point: " << point.transpose() << " centorid_: " << accessor->second->centorid_.transpose();
    }
  });
  //LOG(INFO)<<"raw point num: "<<input_cloud_ptr->size()<<" voxel num: "<<voxel_array_ptr_->size();
  // 表面协方差估计
  cloud_DS_ptr->resize(voxel_array_ptr_->size());
  cloud_cov_ptr->resize(voxel_array_ptr_->size());
  size_t point_with_cov_count = 0;
  //联合周围26个体素，计算每个体素的协方差
  tbb::parallel_for(tbb::blocked_range<size_t>(0, voxel_array_ptr_->size()),[&, this](tbb::blocked_range<size_t> r) 
  {
    for (size_t i = r.begin(); i < r.end(); ++i) 
    {
      cloud_DS_ptr->points[i].getVector3fMap() =voxel_array_ptr_->at(i)->centorid_.cast<float>();
      cloud_cov_ptr->points[i].getVector3fMap() =voxel_array_ptr_->at(i)->centorid_.cast<float>();

      Eigen::Matrix3d modified_cov = Eigen::Matrix3d::Zero();
      size_t points_num = 0;
      size_t all_points_num = 0;//!YEL
      Eigen::Vector3d points_sum = Eigen::Vector3d::Zero();
      Eigen::Matrix3d cov_sum = Eigen::Matrix3d::Zero();

      //printf("search_range_.size():%d\n",search_range_.size());//27
      for (size_t j = 0; j < search_range_.size(); ++j) 
      {
        Eigen::Vector3d near_point =voxel_array_ptr_->at(i)->centorid_ + search_range_[j];
        size_t hash_idx = ComputeHashIndex(near_point,R_w_be,t_w_be);

        MyAccessor accessor;
        if (voxel_map_ptr_->find(accessor, hash_idx)) 
        {
          points_sum += accessor->second->centorid_;
          cov_sum += accessor->second->centorid_ *accessor->second->centorid_.transpose();
          points_num++;
          all_points_num = all_points_num + accessor->second->N_; //!YEL
          cloud_cov_ptr->points[i].features.set(j); //!YEL
        }
        else
          cloud_cov_ptr->points[i].features.reset(j); //!YEL
      }

      //std::cout << "features: " << cloud_cov_ptr->points[i].features << std::endl;
      //! 周围26个体素，至少其中6个体素有点才会真正计算协方差！体素不够的，协方差就不计算了，都是0
      if (points_num >= min_points_per_grid_ ) //!YEL || all_points_num > 40
      {
        Eigen::Vector3d centroid =points_sum / static_cast<double>(points_num);
        Eigen::Matrix3d cov = (cov_sum - points_sum * centroid.transpose()) /(static_cast<double>(points_num) - 1.0);

        Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU | Eigen::ComputeFullV);

        Eigen::Vector3d values(1, 1, 1e-3);

        modified_cov =svd.matrixU() * values.asDiagonal() * svd.matrixV().transpose();

        point_with_cov_count++;
      }

      cloud_cov_ptr->points[i].cov[0] = modified_cov(0, 0);
      cloud_cov_ptr->points[i].cov[1] = modified_cov(0, 1);
      cloud_cov_ptr->points[i].cov[2] = modified_cov(0, 2);
      cloud_cov_ptr->points[i].cov[3] = modified_cov(1, 1);
      cloud_cov_ptr->points[i].cov[4] = modified_cov(1, 2);
      cloud_cov_ptr->points[i].cov[5] = modified_cov(2, 2);
    }
  });

  frame_count_++;
  double current_precent = (static_cast<double>(point_with_cov_count) /
                            static_cast<double>(cloud_cov_ptr->size())) *
                           100.0;
  ava_precent_ +=
      (current_precent - ava_precent_) / static_cast<double>(frame_count_);

  //LOG(INFO) << "point with cov size: " << point_with_cov_count
  //          << " total points: " << cloud_cov_ptr->size()
  //          << " precent: " << current_precent
  //          << ", ava_precent: " << ava_precent_;

  // reset voxel_map and voxel_array

}
